//
// Created by zhanglei on 2023/12/18.
//

#ifndef SLAM_IN_AUTO_DRIVING_IO_UTILS_H
#define SLAM_IN_AUTO_DRIVING_IO_UTILS_H

#include <fstream>
#include <functional>
#include <utility>

/*#include "common/dataset_type.h"
#include "common/global_flags.h"*/
#include "common/gnss.h"
#include "common/imu.h"
#include "common/lidar_utils.h"

#include "common/math_utils.h"
#include "common/message_def.h"
#include "common/odom.h"

#include "ch3/utm_convert.h"

#include "common/read_data.h"

namespace sad {

/**
 * 读取本书提供的数据文本文件，并调用回调函数
 * 数据文本文件主要提供IMU/Odom/GNSS读数
 */
class TxtIO {
   public:
    TxtIO(const std::string &file_path) : fin(file_path) {}

    /// 定义回调函数
    using IMUProcessFuncType = std::function<void(const IMU &)>;
    using OdomProcessFuncType = std::function<void(const Odom &)>;
    using GNSSProcessFuncType = std::function<void(const GNSS &)>;

    TxtIO &SetIMUProcessFunc(IMUProcessFuncType imu_proc) {
        imu_proc_ = std::move(imu_proc);
        return *this;
    }

    TxtIO &SetOdomProcessFunc(OdomProcessFuncType odom_proc) {
        odom_proc_ = std::move(odom_proc);
        return *this;
    }

    TxtIO &SetGNSSProcessFunc(GNSSProcessFuncType gnss_proc) {
        gnss_proc_ = std::move(gnss_proc);
        return *this;
    }

    // 遍历文件内容，调用回调函数
    void Go();

   private:
    std::ifstream fin;
    IMUProcessFuncType imu_proc_;
    OdomProcessFuncType odom_proc_;
    GNSSProcessFuncType gnss_proc_;
};

/**
 * ROSBAG IO
 * 指定一个包名，添加一些回调函数，就可以顺序遍历这个包
 */
class KaistCsvIO {
   public:
    KaistCsvIO(const std::string &file_path, const std::string &data_path) : fin(data_path) {
        file_path_ = file_path;
    }

    /// 定义回调函数
    using IMUProcessFuncType = std::function<void(const IMU &)>;
    using OdomProcessFuncType = std::function<void(const Odom &)>;
    using GNSSProcessFuncType = std::function<void(const GNSS &)>;

    using Scan2DProcessFuncType = std::function<void(const Scan2d &)>;

    using PointCloud3DFuncType = std::function<void(const CloudPtr &)>;
    using SimplePointCloud3DFuncType = std::function<void(const SimpleCloudPtr &)>;

    KaistCsvIO &SetIMUProcessFunc(IMUProcessFuncType imu_proc) {
        imu_proc_ = std::move(imu_proc);
        return *this;
    }

    KaistCsvIO &SetOdomProcessFunc(OdomProcessFuncType odom_proc) {
        odom_proc_ = std::move(odom_proc);
        return *this;
    }

    KaistCsvIO &SetGNSSProcessFunc(GNSSProcessFuncType gnss_proc) {
        gnss_proc_ = std::move(gnss_proc);
        return *this;
    }

    KaistCsvIO &SetScan2DProcessFunc(Scan2DProcessFuncType scan2d_proc) {
        scan2d_proc_ = std::move(scan2d_proc);
        return *this;
    }

    KaistCsvIO &SetPointCloud3DProcessFunc(PointCloud3DFuncType cloud3d_proc) {
        cloud3d_proc_ = std::move(cloud3d_proc);
        return *this;
    }

    KaistCsvIO &SetSimplePointCloudProcessFunc(SimplePointCloud3DFuncType cloud_proc) {
        simple_cloud_proc_ = std::move(cloud_proc);
        return *this;
    }

    // 遍历文件内容，调用回调函数
    void Go();

    void CleanProcessFunc() { 
        /*imu_proc_.clear(); 
        odom_proc_.clear(); 
        gnss_proc_.clear(); 
        scan2d_proc_.clear(); 
        cloud3d_proc_.clear(); 
        simple_cloud_proc_.clear();*/ 
    }

   private:
    std::ifstream fin;
    IMUProcessFuncType imu_proc_;
    OdomProcessFuncType odom_proc_;
    GNSSProcessFuncType gnss_proc_;
    Scan2DProcessFuncType scan2d_proc_;
    PointCloud3DFuncType cloud3d_proc_;
    SimplePointCloud3DFuncType simple_cloud_proc_;

    read_data* read_input_data;
    double last_left = 0;
    double last_right = 0;
    std::string file_path_;

    bool used_simple_point = true;
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_IO_UTILS_H
